#include "AGVThreadController.h"
#include "math.h"
#include <QtConcurrent/qtconcurrentrun.h>
#include <cstdlib>
#include <exception>
#include <mutex>
#include <qcoreapplication.h>
#include <qfuturewatcher.h>
#include <sstream>

#include "log.h"
#include "spdlog/logger.h"
#include "spdlog/sinks/rotating_file_sink.h"
#include "spdlog/spdlog.h"
#include "struct.h"

static struct _user_type_register {
  public:
  _user_type_register() {
    int ret;
    ret = qRegisterMetaType<secondlocatecalc_result_t>(
        "secondlocatecalc_result_t");
    qDebug() << __PRETTY_FUNCTION__ << "register secondlocatecalc_result_t"
             << ret;

    ret =
        qRegisterMetaType<initialposition_result_t>("initialposition_result_t");
    qDebug() << __PRETTY_FUNCTION__ << "register initialposition_result_t"
             << ret;

    ret = qRegisterMetaType<SecondLocateParams>("SecondLocateParams");
    qDebug() << __PRETTY_FUNCTION__ << "register SecondLocateParams" << ret;

    ret = qRegisterMetaType<BackToZeroParams>("BackToZeroParams");
    qDebug() << __PRETTY_FUNCTION__ << "register BackToZeroParams" << ret;

    ret = qRegisterMetaType<CalculateMapAngleParams>("CalculateMapAngleParams");
    qDebug() << __PRETTY_FUNCTION__ << "register CalculateMapAngleParams" << ret;

    ret = qRegisterMetaType<CalculateMapAngleResponse>("CalculateMapAngleResponse");
    qDebug() << __PRETTY_FUNCTION__ << "register CalculateMapAngleResponse" << ret;
  }
} __user_type_register;

AGVThreadWorker::AGVThreadWorker(QObject *parent) : QObject(parent) {
  auto default_logger = spdlog::default_logger();
  auto sinks = default_logger->sinks();

  std::string log_file = ROOT_DIR "logs/agv_worker.log";
  auto another_rotating_sink =
      std::make_shared<spdlog::sinks::rotating_file_sink_mt>(
          log_file, 1024 * 1024 * 2, 100);
  another_rotating_sink->set_level(spdlog::level::trace);
  sinks.push_back(another_rotating_sink);

  this->logger_ = std::make_shared<spdlog::logger>(std::string{"AGV"},
                                                   sinks.begin(), sinks.end());
  this->logger_->set_level(spdlog::level::trace);

  this->logger_->flush_on(spdlog::level::trace);

  // logger_->register_logger(this->logger_);

  this->logger_->info(" ");
  this->logger_->info("**************************");
  this->logger_->info("**** AGV Worker Start ****");
  this->logger_->info("**************************");
}

AGVThreadWorker::~AGVThreadWorker() {
  logger_->trace("enter {}", __PRETTY_FUNCTION__);

  slot_StopAgvSystem();

  if (!movetotarget_thread_future_watcher_.isFinished()) {
    logger_->trace("wait for movetotarget_thread_future_watcher_ finished");
    movetotarget_thread_future_watcher_.waitForFinished();
    logger_->trace("wait for movetotarget_thread_future_watcher_ finished end");
  }

  if (!secondlocatecalc_future_watcher_.isFinished()) {
    logger_->trace("wait for secondlocatecalc_future_watcher_ finished");
    secondlocatecalc_future_watcher_.waitForFinished();
    logger_->trace("wait for secondlocatecalc_future_watcher_ finished end");
  }

  if (!initialposition_future_watcher_.isFinished()) {
    logger_->trace("wait for initialposition_future_watcher_ finished");
    initialposition_future_watcher_.waitForFinished();
    logger_->trace("wait for initialposition_future_watcher_ finished end");
  }

  if (!calculate_map_angle_future_watcher_.isFinished()) {
    logger_->trace("wait for calculate_map_angle_future_watcher_ finished");
    calculate_map_angle_future_watcher_.waitForFinished();
    logger_->trace("wait for calculate_map_angle_future_watcher_ finished end");
  }

  logger_->trace("exit {}", __PRETTY_FUNCTION__);

  this->logger_->info("**************************");
  this->logger_->info("***** AGV Worker End *****");
  this->logger_->info("**************************");
}

void AGVThreadWorker::slot_do_init() {
  connect(&initialposition_future_watcher_,
          &QFutureWatcher<initialposition_result_t>::finished, this, [this]() {
            auto result = initialposition_future_watcher_.result();

            auto level = result.err_code == ErrCode::NONE ? spdlog::level::info
                                                          : spdlog::level::err;
            SPDLOG_LOGGER_CALL(logger_, level,
                               "initialposition_future_watcher_ "
                               "finished, result: {}, err_code: "
                               "{}",
                               result.result, (int)result.err_code);
            // logger_->info(
            //     "initialposition_future_watcher_ finished, result: {}, "
            //     "err_code: {}",
            //     result.result, (int)result.err_code);
            emit _sig_InitialPositionResult(result);

            {
              std::lock_guard guard(is_initialposition_running_mutex_);
              is_initialposition_running_ = false;
            }
          });

  connect(&secondlocatecalc_future_watcher_,
          &QFutureWatcher<secondlocatecalc_result_t>::finished, this, [this]() {
            auto result = secondlocatecalc_future_watcher_.result();

            auto level = result.err_code == ErrCode::NONE ? spdlog::level::info
                                                          : spdlog::level::err;
            SPDLOG_LOGGER_CALL(
                logger_, level,
                "secondlocatecalc_future_watcher_ finished, "
                "result: axis1_inc: {}, axis2_inc: {}, err_code: "
                "{}",
                result.axis1_inc, result.axis2_inc, (int)result.err_code);
            // logger_->info("secondlocatecalc_future_watcher_ finished, result:
            // "
            //               "axis1_inc: {}, axis2_inc: {}, err_code: {}",
            //               result.axis1_inc, result.axis2_inc,
            //               (int)result.err_code);
            emit _sig_SecondLocateMovePlc(result);

            {
              std::lock_guard guard(is_secondlocatecalc_running_mutex_);
              is_secondlocatecalc_running_ = false;
            }
          });
  
  connect(&calculate_map_angle_future_watcher_,
          &QFutureWatcher<CalculateMapAngleResponse>::finished, this, [this]() {
            auto result = calculate_map_angle_future_watcher_.result();

            std::stringstream ss;
            ss << "[";
            for (int i = 0; i < 4; ++i) {
              ss << result.points[i];
              if (i < 3) {
                ss << ", ";
              }
            }
            ss << "]";

            auto level = result.errCode == ErrCode::NONE ? spdlog::level::info
                                                          : spdlog::level::err;
            SPDLOG_LOGGER_CALL(
                logger_, level,
                "calculate_map_angle_future_watcher_ finished, "
                "result: points: {}, errCode: {}",
                ss.str(), (int)result.errCode);

            emit _sig_CalculateMapAngleResult(result);

            {
              std::lock_guard guard(is_calculate_map_angle_running_mutex_);
              is_calculate_map_angle_running_ = false;
            }
          });

  connect(&movetotarget_thread_future_watcher_, &QFutureWatcher<void>::finished,
          this, [this]() {
            logger_->info("movetotarget_thread_future_watcher_ finished");

            {
              std::lock_guard guard(is_movetotarget_thread_running_mutex_);
              is_movetotarget_thread_running_ = false;
            }
          });

  logger_->trace("{}", __PRETTY_FUNCTION__);
  int AGV_mode = 1;
  agv_system_ = new AGVSystem(AGV_mode);
  agv.setPermission();
}

/*
void AGVThreadWorker::slot_AgvStartMove(double dist, double speed, int type) {
  logger_->trace("{}, dist: {}, speed: {}, type: {}", __PRETTY_FUNCTION__, dist,
                speed, type);

  //根据运动状态判断是否发送
  nlohmann::json responseJson = agv.getSpeed();
  if (responseJson["is_stop"] == true) //未在运动
  {
    // speed和dist安全性
    if (dist == 0 || speed == 0) {
      qDebug() << __FUNCTION__ << "AGV移动失败, 距离或速度为0";
      return;
    }

    if (type == 0) //走直线
    {
      agv_system_->agvMove(dist, speed);
    } else if (type == 1) //走旋转
    {
      agv_system_->agvRotate(dist, speed);
    }
  } else {
    qDebug() << __FUNCTION__ << "AGV移动失败, AGV正在运动";
  }
}
*/

void AGVThreadWorker::slot_AgvStartLineMove(double inc_m, double speed_m_s) {
  logger_->trace("{}, inc_m: {:.6f}, speed_m_s: {:.9f}", __PRETTY_FUNCTION__,
                 inc_m, speed_m_s);

  try {
    agv_system_->agvMove(inc_m, speed_m_s);
  } catch (const std::exception &e) {
    logger_->error("exception in slot_AgvStartLineMove: {}", e.what());
  }
}

void AGVThreadWorker::slot_AgvStartRotateMove(double inc_rad,
                                              double speed_rad_s) {
  logger_->trace("{}, inc_rad: {:.6f}, speed_rad_s: {:.9f}",
                 __PRETTY_FUNCTION__, inc_rad, speed_rad_s);

  try {
    agv_system_->agvRotate(inc_rad, speed_rad_s);
  } catch (const std::exception &e) {
    logger_->error("exception in slot_AgvStartRotateMove: {}", e.what());
  }
}

void AGVThreadWorker::slot_AgvStopMove() {
  //根据运动状态判断是否发送
  logger_->trace("{}", __PRETTY_FUNCTION__);
  try {
    agv_system_->agvStop();
  } catch (const std::exception &e) {
    logger_->error("exception in slot_AgvStopMove: {}", e.what());
  }

  return;
  //   nlohmann::json responseJson = agv.getSpeed();
  //   std::cout << responseJson.contains("is_stop") << endl;
  //   if (responseJson["is_stop"] == false) {
  //     agv_system_->agvStop();
  //     qDebug() << __FUNCTION__ << "AGV停止点动移动!";
  //   }
}

void AGVThreadWorker::slot_AgvInitStart(double x_m, double y_m, double angle,
                                        int timeout_ms) {
  if (!initialposition_future_watcher_.isFinished()) {
    logger_->error("initialposition_future_watcher_ is not finished");
    return;
  }

  if (!initialposition_future_.isFinished()) {
    logger_->error("initialposition_future_ is not finished");
    return;
  }

  logger_->info("starting initialposition thread");

  initialposition_future_ = QtConcurrent::run(
      [this, x_m, y_m, angle, timeout_ms]() -> initialposition_result_t {
        logger_->trace(
            "QtConcurrent::run enter agv_system_->initialPosition(), x_m: "
            "{:.6f}, y_m: {:.6f}, angle: {}, timeout: {}",
            x_m, y_m, angle, timeout_ms);

        {
          std::lock_guard guard(is_initialposition_running_mutex_);
          is_initialposition_running_ = true;
        }

        initialposition_result_t res;

        try {
          auto result = agv_system_->initialPosition(x_m, y_m, angle,
                                                     timeout_ms); // timeout 10s
          res.result = result.score;
          res.err_code = result.errCode;
        } catch (const std::exception &e) {
          res.err_code = ErrCode::EXCEPTION;
          res.result = 0.0;
          logger_->error("exception in initialPosition!!!, {}", e.what());
        }

        logger_->trace("QtConcurrent::run exit agv_system_->initialPosition(); "
                       "result: {}, err_code: {}",
                       res.result, (int)res.err_code);
        return res;
      });
  initialposition_future_watcher_.setFuture(initialposition_future_);
}

void AGVThreadWorker::slot_AgvInitStop() {
  logger_->trace("{}", __PRETTY_FUNCTION__);

  try {
    agv_system_->finishInitialPosition();
  } catch (const std::exception &e) {
    logger_->error("exception in finishInitialPosition: {}", e.what());
  }
}

void AGVThreadWorker::slot_AgvBackToZero(BackToZeroParams params) {
  logger_->trace("{}, x_mm: {}, y_mm: {}, threshold: {}", __PRETTY_FUNCTION__,
                 params.x_mm, params.y_mm, params.threshold);
  slot_AgvStartMoveToTargetThread();

  try {
    agv_system_->backToZero(params);
  } catch (const std::exception &e) {
    logger_->error("exception in slot_AgvBackToZero!!!, {}", e.what());
  }
}

void AGVThreadWorker::slot_AgvStartBackChange(double x_mm, double y_mm) {
  logger_->trace("{}, x_mm: {}, y_mm: {}", __PRETTY_FUNCTION__, x_mm, y_mm);
  slot_AgvStartMoveToTargetThread();

  try {
    bool ret = agv_system_->startBackChange(x_mm, y_mm);
    if (!ret) {
      logger_->error("agv_system_->startBackChange failed!!!");
    }
  } catch (const std::exception &e) {
    logger_->error("exception in slot_AgvStartBackChange!!!, {}", e.what());
  }
}

void AGVThreadWorker::slot_AgvMoveToTarget(double _x_pos, double _y_pos) {
  logger_->trace("{}, x_pos: {} mm, y_pos: {} mm", __PRETTY_FUNCTION__, _x_pos,
                 _y_pos);
  //如果正在运动 不执行
  try {
    nlohmann::json responseJson = agv.getSpeed();
    if (responseJson["is_stop"] == true) {
      bool ret = agv_system_->setTarget(_x_pos, _y_pos);
      if (ret == false) {
        logger_->error("{} AGV setTarget failed", __FUNCTION__);
      } else {
        logger_->info("{} AGV setTarget success", __FUNCTION__);
      }
    } else {
      logger_->error("{} AGV not stopped, setTarget Failed", __FUNCTION__);
    }
  } catch (const std::exception &e) {
    logger_->error("exception in slot_AgvMoveToTarget!!!, {}", e.what());
  }
}

void AGVThreadWorker::slot_AgvStartMoveToTargetThread() {
  if (!movetotarget_thread_future_watcher_.isFinished()) {
    logger_->warn("movetotarget_thread_future_watcher_ not finished, start "
                  "thread failed");
    return;
  }

  if (!movetotarget_thread_future_.isFinished()) {
    logger_->error(
        "movetotarget_thread_future_ not finished, start thread failed");
    return;
  }

  logger_->trace("starting movetotarget thread");
  movetotarget_thread_future_ = QtConcurrent::run([this]() -> void {
    logger_->trace("QtConcurrent::run enter agv_system_->moveToTarget();");

    {
      std::lock_guard guard(is_movetotarget_thread_running_mutex_);
      is_movetotarget_thread_running_ = true;
    }

    try {
      agv_system_->moveToTarget();
    } catch (const std::exception &e) {
      logger_->error("exception in moveToTarget!!!, {}", e.what());
    }

    logger_->trace("QtConcurrent::run exit agv_system_->moveToTarget();");
  });
  movetotarget_thread_future_watcher_.setFuture(movetotarget_thread_future_);
}

void AGVThreadWorker::slot_StopAgvSystem() {
  logger_->trace("{}", __PRETTY_FUNCTION__);

  try {
    agv_system_->stopSystem();
  } catch (const std::exception &e) {
    logger_->error("exception in stopSystem!!!, {}", e.what());
  }
}

void AGVThreadWorker::slot_SecondLocateCalc(SecondLocateParams params) {
  if (!secondlocatecalc_future_watcher_.isFinished()) {
    logger_->error(
        "secondlocatecalc_future_watcher_ not finished, start thread failed");
    return;
  }

  if (!secondlocatecalc_future_.isFinished()) {
    logger_->error(
        "secondlocatecalc_future_ not finished, start thread failed");
    return;
  }

  logger_->trace("starting secondlocatecalc thread");
  secondlocatecalc_future_ =
      QtConcurrent::run([this, params]() -> secondlocatecalc_result_t {
        logger_->trace("QtConcurrent::run enter agv_system_->secondLocate();");

        {
          std::lock_guard guard(is_secondlocatecalc_running_mutex_);
          is_secondlocatecalc_running_ = true;
        }

        secondlocatecalc_result_t return_result;
        try {
          auto result = agv_system_->secondLocate(params); // timeout 10s

          return_result.axis1_inc = result.axis1;
          return_result.axis2_inc = result.axis2;
          return_result.err_code = result.errCode;
        } catch (const std::exception &e) {
          return_result.axis1_inc = 0;
          return_result.axis2_inc = 0;
          return_result.err_code = (ErrCode::EXCEPTION);
          logger_->error("exception in secondlocate!!!, {}", e.what());
        }

        logger_->trace("QtConcurrent::run exit agv_system_->secondLocate(); "
                       "axis1_inc: {}, axis2_inc: {}, err_code: {}",
                       return_result.axis1_inc, return_result.axis2_inc,
                       (int)return_result.err_code);

        return return_result;
      });
  secondlocatecalc_future_watcher_.setFuture(secondlocatecalc_future_);
}

void AGVThreadWorker::slot_CleanUpAgvTcp() {
  logger_->trace("{}", __PRETTY_FUNCTION__);

  try {
    agv_system_->cleanAgvTcp();
  } catch (const std::exception &e) {
    logger_->error("exception in cleanAgvTcp!!!, {}", e.what());
  }
}

void AGVThreadWorker::slot_CalculateMapAngle(CalculateMapAngleParams params) {
  if (!calculate_map_angle_future_watcher_.isFinished()) {
    logger_->error(
        "calculate_map_angle_future_watcher_ not finished, start thread failed");
    return;
  }

  if (!calculate_map_angle_future_.isFinished()) {
    logger_->error(
        "calculate_map_angle_future_ not finished, start thread failed");
    return;
  }

  logger_->trace("starting calculate_map_angle thread, binaryThreshold: {}, "
                 "maxRadius: {}, timeOut: {}",  params.binaryThreshold, params.maxRadius, params.timeOut);

  calculate_map_angle_future_ =
      QtConcurrent::run([this, params]() -> CalculateMapAngleResponse {
        logger_->trace("QtConcurrent::run enter agv_system_->calculateMapAngle();");

        {
          std::lock_guard guard(is_calculate_map_angle_running_mutex_);
          is_calculate_map_angle_running_ = true;
        }

        CalculateMapAngleResponse return_result;
        try {
          auto result = agv_system_->calculateMapAngle(params); // timeout 10s

          return_result = result;
        } catch (const std::exception &e) {
          return_result.points[0] = 0;
          return_result.points[1] = 0;
          return_result.points[2] = 0;
          return_result.points[3] = 0;
          return_result.errCode = (ErrCode::EXCEPTION);
          logger_->error("exception in calculateMapAngle!!!, {}", e.what());
        }

        logger_->trace("QtConcurrent::run exit agv_system_->calculateMapAngle(); "
                       "points: [{}, {}, {}, {}], errCode: {}",
                       return_result.points[0], return_result.points[1],
                       return_result.points[2], return_result.points[3],
                       (int)return_result.errCode);

        return return_result;
      });
  calculate_map_angle_future_watcher_.setFuture(calculate_map_angle_future_);
}

AGVThreadController::AGVThreadController(QObject *parent) : QObject(parent) {
  thread_ = new QThread(this);
  worker_ = new AGVThreadWorker(nullptr);
  worker_->moveToThread(thread_);
  connect(thread_, &QThread::finished, worker_, &QObject::deleteLater);

  // Controller to Worker
  connect(this, &AGVThreadController::_sig_do_init, worker_,
          &AGVThreadWorker::slot_do_init);

  // connect(this, &AGVThreadController::_sig_AgvStartMove, worker_,
  //         &AGVThreadWorker::slot_AgvStartMove);
  connect(this, &AGVThreadController::_sig_AgvStartLineMove, worker_,
          &AGVThreadWorker::slot_AgvStartLineMove);
  connect(this, &AGVThreadController::_sig_AgvStartRotateMove, worker_,
          &AGVThreadWorker::slot_AgvStartRotateMove);
  connect(this, &AGVThreadController::_sig_AgvStopMove, worker_,
          &AGVThreadWorker::slot_AgvStopMove);
  connect(this, &AGVThreadController::_sig_AgvInitStart, worker_,
          &AGVThreadWorker::slot_AgvInitStart);
  connect(this, &AGVThreadController::_sig_AgvInitStop, worker_,
          &AGVThreadWorker::slot_AgvInitStop);
  connect(this, &AGVThreadController::_sig_AgvBackToZero, worker_,
          &AGVThreadWorker::slot_AgvBackToZero);
  connect(this, &AGVThreadController::_sig_AgvStartBackChange, worker_,
          &AGVThreadWorker::slot_AgvStartBackChange);
  connect(this, &AGVThreadController::_sig_StopAgvSystem, worker_,
          &AGVThreadWorker::slot_StopAgvSystem);
  connect(this, &AGVThreadController::_sig_AgvMoveToTarget, worker_,
          &AGVThreadWorker::slot_AgvMoveToTarget);
  connect(this, &AGVThreadController::_sig_AgvStartMoveToTargetThread, worker_,
          &AGVThreadWorker::slot_AgvStartMoveToTargetThread);
  connect(this, &AGVThreadController::_sig_SecondLocateCalc, worker_,
          &AGVThreadWorker::slot_SecondLocateCalc);
  connect(this, &AGVThreadController::_sig_CleanUpAgvTcp, worker_,
          &AGVThreadWorker::slot_CleanUpAgvTcp);
  connect(this, &AGVThreadController::_sig_CalculateMapAngle, worker_,
          &AGVThreadWorker::slot_CalculateMapAngle);

  // Worker To Controller
  connect(worker_, &AGVThreadWorker::_sig_CommomMessage, this,
          &AGVThreadController::sig_CommomMessage);
  connect(worker_, &AGVThreadWorker::_sig_SecondLocateMovePlc, this,
          &AGVThreadController::sig_SecondLocateMovePlc);
  connect(worker_, &AGVThreadWorker::_sig_InitialPositionResult, this,
          &AGVThreadController::sig_InitialPositionResult);
  connect(worker_, &AGVThreadWorker::_sig_CalculateMapAngleResult, this,
          &AGVThreadController::sig_CalculateMapAngleResult);

  thread_->start();
  emit this->_sig_do_init();
}

AGVThreadController::~AGVThreadController() {
  spdlog::trace("enter {}", __PRETTY_FUNCTION__);
  thread_->quit();
  bool ret = thread_->wait(3000);
  // thread_->wait();
  spdlog::trace("quit {}, wait over", __PRETTY_FUNCTION__);

  if (!ret) {
    spdlog::critical("wait timeout, kill process");
    spdlog::default_logger()->flush();
    abort();
  }
}

// void AGVThreadController::AgvStartMove(double dist, double speed, int type) {
//   emit this->_sig_AgvStartMove(dist, speed, type);
// }

void AGVThreadController::AgvStartLineMove(double inc_m, double speed_m_s) {
  emit this->_sig_AgvStartLineMove(inc_m, speed_m_s);
}

void AGVThreadController::AgvStartRotateMove(double inc_rad,
                                             double speed_rad_s) {
  emit this->_sig_AgvStartRotateMove(inc_rad, speed_rad_s);
}

void AGVThreadController::AgvStopMove() { emit this->_sig_AgvStopMove(); }

void AGVThreadController::AgvInitStart(double x_m, double y_m, double angle,
                                       int timeout_ms) {
  emit this->_sig_AgvInitStart(x_m, y_m, angle, timeout_ms);
}

void AGVThreadController::AgvInitStop() { emit this->_sig_AgvInitStop(); }

void AGVThreadController::AgvBackToZero(BackToZeroParams params) {
  emit this->_sig_AgvBackToZero(params);
}

void AGVThreadController::AgvStartBackChange(double x_mm, double y_mm) {
  emit this->_sig_AgvStartBackChange(x_mm, y_mm);
}

void AGVThreadController::StopAgvSystem() { emit this->_sig_StopAgvSystem(); }

void AGVThreadController::AgvMoveToTarget(double _x_pos, double _y_pos) {
  emit this->_sig_AgvMoveToTarget(_x_pos, _y_pos);
}

void AGVThreadController::AgvStartMoveToTargetThread() {
  emit this->_sig_AgvStartMoveToTargetThread();
}

void AGVThreadController::SecondLocateCalc(SecondLocateParams param) {
  emit this->_sig_SecondLocateCalc(param);
}

void AGVThreadController::CleanUpAgvTcp() { emit this->_sig_CleanUpAgvTcp(); }

void AGVThreadController::CalculateMapAngle(CalculateMapAngleParams params) {
  emit this->_sig_CalculateMapAngle(params);
}

